#include "StdAfx.h"
#include "MTDriveServer.h"
#include <iostream>
#include <math.h>

CMTDriveServer::CMTDriveServer(void):	
	m_pOSCPacket(NULL),
	m_TcpServer(NULL),
	m_UdpServer(NULL),
	m_cmtSensor(NULL),
	m_bOSC(false),
	m_bUDP(false),
	m_bRunThread(true)
{
}

CMTDriveServer::~CMTDriveServer(void)
{
}

bool CMTDriveServer::InitServer(const char *pcHostName, long lPort, bool bOSC, bool bUDP, long lWidth, long lHeight)
{
	if(m_TcpServer)
		ShutDown();
	if(m_UdpServer)
		ShutDown();	

	ZeroMemory(&m_cmtCursor, sizeof(CMTCursor));
	ZeroMemory(&m_cmtCursorChar, sizeof(CMTCursorChar));
	ZeroMemory(&m_cmtSession, sizeof(CMTSession));

	strncpy(m_cmtSession.cstrHost, pcHostName, strlen(pcHostName) +1);
	m_cmtSession.lPort = lPort;
	m_cmtSession.lFseq = 0;
	sprintf_s(m_cmtSession.cPort, MAX_FLOAT_SIZE, "%d", lPort);
	sprintf_s(m_cmtSession.cFseq, MAX_FLOAT_SIZE, "%d", 0);

	m_lWidth = lWidth;
	m_lHeight = lHeight;
	m_strXML.clear();

	if(bOSC)
		m_pOSCPacket = new osc::OutboundPacketStream(m_buffer, MAX_BUF_SIZE);
	m_bOSC = bOSC;

	if(bUDP)
	{
		IpEndpointName host( pcHostName, lPort);
		m_UdpServer = new UdpTransmitSocket(host);
	}
	else
	{
		m_TcpServer = new TcpServer();
		m_TcpServer->Start(lPort);
	}
	m_bUDP = bUDP;	

	// Create a sensor without setting a callback handler, since we are going to pull the current device state by ourself.
	//
	m_cmtSensor = SMT_Open(0, lWidth, lHeight, 0, 0);
	if(!m_cmtSensor)
	{
		OutputDebugString(_T("couldn't connect to default SMK device\n"));
		ShutDown();
		return false;
	}	

	// Worker thread calling the SMTUpdate() function to force calls of our callback.
	//
	m_bRunThread = true;
	m_thread = ::CreateThread(NULL, NULL, CMTDriveServer::UpdataSensor, this, NULL, NULL);
	if(!m_thread)
	{
		ShutDown();
		_ASSERTE(0);
		return false;
	}

	return true;
}

bool CMTDriveServer::ShutDown()
{
	// Stop the worker thread as first.
	//
	m_bRunThread = false;
	Sleep(500);

	if(m_cmtSensor)
	{
		SMT_Close(m_cmtSensor);
		m_cmtSensor = NULL;
	}

	if(m_pOSCPacket)
	{
		delete m_pOSCPacket;
		m_pOSCPacket = NULL;
	}

	if(m_TcpServer)
	{
		m_TcpServer->Stop();
		delete m_TcpServer;
		m_TcpServer = NULL;
	}
	if(m_UdpServer)
	{
		delete m_UdpServer;
		m_UdpServer = NULL;
	}

	if(m_thread)
	{
		::CloseHandle(m_thread);
		m_thread = NULL;
	}
	
	return true;
}

DWORD WINAPI CMTDriveServer::UpdataSensor(void *pData)
{
	if(!pData)
	{
		_ASSERTE(0);
		return 0;
	}

	CMTDriveServer *pThis = static_cast<CMTDriveServer*>(pData);
	bool run = true;
	while(pThis->m_bRunThread && run)
	{
		pThis->m_vCursors.clear();

		if(!SMT_Update(pThis->m_cmtSensor))
			run = false;

		// iterate on cursor list, and draw each cursor that's currently in SMT_CURSOR_DOWN state
		SMT_CURSOR c = SMT_GetSensorCursors(pThis->m_cmtSensor);
		while(c)
		{
			// skip SMT_CURSOR_UP cursors waiting for multiclick
			if (SMT_GetCursorState(c)==SMT_CURSOR_DOWN)
			{
				pThis->m_vCursors.push_back(c);
			}
			c = SMT_GetCursorNext(c);
		}

		pThis->UpdateCursor();
	}

	return 0;
}

bool CMTDriveServer::UpdateCursorXML()
{
	if(m_bOSC)
		return false;	

	// OSCPacket begin
	//
	m_strXML.clear();
	m_strXML = "<OSCPACKET ADDRESS=\"";
	m_strXML += m_cmtSession.cstrHost;
	m_strXML += "\" PORT=\"";
	m_strXML += m_cmtSession.cPort;
	m_strXML += "\" TIME=\"1\">";

	// Message "set"
	//		
	for(size_t s=0; s < m_vCursors.size(); ++s)
	{
		m_cmtCursor.ID = SMT_GetCursorID(m_vCursors[s]);
		m_cmtCursor.dx = 0; //m_cmtCursor.x - (fX / 1000.0);
		m_cmtCursor.dy = 0; //m_cmtCursor.y - (fY / 1000.0);
		m_cmtCursor.x = ((float)SMT_GetCursorX(m_vCursors[s]) / 1000.0);
		m_cmtCursor.y = ((float)SMT_GetCursorY(m_vCursors[s]) / 1000.0);
		m_cmtCursor.m = 0; //sqrtf((m_cmtCursor.dx * m_cmtCursor.dx) + (m_cmtCursor.dy * m_cmtCursor.dy));	// Motion acceleration

		sprintf_s(m_cmtCursorChar.ccID, MAX_FLOAT_SIZE, "%d", m_cmtCursor.ID);
		sprintf_s(m_cmtCursorChar.cX, MAX_FLOAT_SIZE, "%f", m_cmtCursor.x);
		sprintf_s(m_cmtCursorChar.cY, MAX_FLOAT_SIZE, "%f", m_cmtCursor.y);
		sprintf_s(m_cmtCursorChar.cDX, MAX_FLOAT_SIZE, "%f", m_cmtCursor.dx);
		sprintf_s(m_cmtCursorChar.cDY, MAX_FLOAT_SIZE, "%f", m_cmtCursor.dy);
		sprintf_s(m_cmtCursorChar.cM, MAX_FLOAT_SIZE, "%f", m_cmtCursor.m);

		m_strXML += "\n<MESSAGE NAME=\"/tuio/2Dcur\">";
		m_strXML += "\n<ARGUMENT TYPE=\"s\" VALUE=\"set\" />";
		m_strXML += "\n<ARGUMENT TYPE=\"i\" VALUE=\"";
		m_strXML += m_cmtCursorChar.ccID;
		m_strXML += "\" />";
		m_strXML += "\n<ARGUMENT TYPE=\"f\" VALUE=\"";
		m_strXML += m_cmtCursorChar.cX;
		m_strXML += "\" />";
		m_strXML += "\n<ARGUMENT TYPE=\"f\" VALUE=\"";
		m_strXML += m_cmtCursorChar.cY;
		m_strXML += "\" />";
		m_strXML += "\n<ARGUMENT TYPE=\"f\" VALUE=\"";
		m_strXML += m_cmtCursorChar.cDX;
		m_strXML += "\" />";
		m_strXML += "\n<ARGUMENT TYPE=\"f\" VALUE=\"";
		m_strXML += m_cmtCursorChar.cDY;
		m_strXML += "\" />";
		m_strXML += "\n<ARGUMENT TYPE=\"f\" VALUE=\"";
		m_strXML += m_cmtCursorChar.cM;
		m_strXML += "\" />";
		m_strXML += "\n</MESSAGE>";
	}

	// "alive" Message
	//
	m_strXML += "\n<MESSAGE NAME=\"/tuio/2Dcur\">\n<ARGUMENT TYPE=\"s\" VALUE=\"alive\" />";
	{
		for(size_t s=0; s < m_vCursors.size(); ++s)
		{
			m_cmtCursor.ID = SMT_GetCursorID(m_vCursors[s]);
			sprintf_s(m_cmtCursorChar.ccID, MAX_FLOAT_SIZE, "%d", m_cmtCursor.ID);
			m_strXML +="\n<ARGUMENT TYPE=\"i\" VALUE=\"";
			m_strXML += m_cmtCursorChar.ccID;
			m_strXML += "\" />";
		}	
	}	
	m_strXML += "\n</MESSAGE>";

	// "fseq" Message
	//
	m_strXML += "\n<MESSAGE NAME=\"/tuio/2Dcur\">";
	m_strXML += "<ARGUMENT TYPE=\"s\" VALUE=\"fseq\" />";
	m_strXML += "\n<ARGUMENT TYPE=\"i\" VALUE=\"";
	m_strXML += m_cmtSession.cFseq;
	m_strXML += "\" />";
	m_strXML += "\n</MESSAGE>";

	// OSCPacket end
	//
	m_strXML += "\n</OSCPACKET>\n";
	m_strXML += '\0';

	//OutputDebugStringA(m_strXML.c_str());

	m_cmtSession.lFseq ++;
	sprintf_s(m_cmtSession.cFseq, MAX_FLOAT_SIZE, "%d", m_cmtSession.lFseq);

	if(m_bUDP)
		m_UdpServer->Send(m_strXML.c_str(), m_strXML.size());
	else
		m_TcpServer->Send(m_strXML.c_str(), m_strXML.size());

	return true;
}

bool CMTDriveServer::UpdateCursorOSC(int iCurID, float fX, float fY)
{
	if(!m_bOSC)
		return false;

	m_pOSCPacket->Clear();
	*m_pOSCPacket << osc::BeginBundle();
	
	m_cmtCursor.ID = iCurID;
	m_cmtCursor.dx = m_cmtCursor.x - (fX / 1000.0);
	m_cmtCursor.dy = m_cmtCursor.y - (fY / 1000.0);
	m_cmtCursor.x = (fX / 1000.0);
	m_cmtCursor.y = (fY / 1000.0);	

	// "set" Message
	//
	m_cmtCursor.m = sqrtf((m_cmtCursor.dx * m_cmtCursor.dx) + (m_cmtCursor.dy * m_cmtCursor.dy));	// Motion acceleration	
	if(!(fX == 0 && fY == 0))
	{
		*m_pOSCPacket << osc::BeginMessage( "/tuio/2Dcur" ) << "set" << iCurID << m_cmtCursor.x << m_cmtCursor.y << m_cmtCursor.dx << m_cmtCursor.dy << m_cmtCursor.m << osc::EndMessage;	
	}

	// "alive" Message
	//
	*m_pOSCPacket << osc::BeginMessage( "/tuio/2Dcur" ) << "alive" << m_cmtCursor.ID << osc::EndMessage;

	// "fseq" Message
	//
	*m_pOSCPacket << osc::BeginMessage( "/tuio/2Dcur" ) << "fseq" << (long)m_cmtSession.lFseq << osc::EndMessage;

	*m_pOSCPacket << osc::EndBundle;

	m_cmtSession.lFseq ++; 
	if(m_pOSCPacket->IsReady())
	{
		if(m_bUDP)
			m_UdpServer->Send(m_pOSCPacket->Data(), m_pOSCPacket->Size());
		else
			m_TcpServer->Send(m_pOSCPacket->Data(), m_pOSCPacket->Size());
	}

	return true;
}
